#include "test.h"
#include "uart.h"
#include "adc.h"
#include "stdint.h"
#include "portdefine.h"
#include "I2C.h"

#include <stdio.h>
#include <string.h>
#include <math.h>

uint16_t MPU_5VMPU1, MPU_5VMPU2, MPU_5VSWC1, MPU_5VSWC2, MPU_LCD1, MPU_LCD2, MPU_BOT_FLOW;

uint16_t SECMOTOR;
uint16_t MPU_AN2_RBATSW;
uint16_t VREF_41;
uint16_t MPU_AN4_SPKR1;
uint16_t MPU_AN5_RBAT;
uint16_t MPU_AN6_LBATSW;
uint16_t MPU_AN7_LBAT;
uint16_t MPU_AN8_VMOTOR;
uint16_t MPU_AN9_VEXT;
uint16_t MPU_AN10_SPKR2;
uint16_t MPU_AN11_TOP_PSI;
uint16_t MPU_AN12_ATMOS_PSI;
uint16_t MPU_AN13_BOT_PSI;
uint16_t MPU_AN14_5VPSI;
uint16_t MPU_ISYSTEM;

static uint16_t PriSpeedV = 0, SlaSpeedV = 0;
static uint32_t PriSpeedV1 = 0, SlaSpeedV1 = 0;
static uint16_t InputPowerCurrent = 0;

static uint8_t PriVState = 0, SlaVState = 0;
static uint8_t PriEnState = 0, TDC5VState = 0;
static uint8_t TDCState = 0, BDCState = 0;
static uint8_t OldPriVState = 0, OldSlaVState = 0, OldPriEnState = 0;
static uint8_t OldTDCState = 0, OldBDCState = 0, OldTDC5VState = 0;
static uint8_t statusByte = 0;
extern uint8_t MotorEnable;

extern uint8_t LeftBatCapcity, RightBatCapcity;
extern uint16_t LeftBatTemp, RightBatTemp;
extern uint16_t MotorFreq;
extern uint16_t MotorFreqArray[10];
extern uint8_t UartReceiveOKFlag;
extern volatile UART_State rxState;
extern uint8_t Usart1RecCount;

extern uint16_t LM73Temperature1, LM73Temperature2;

uint8_t MotorSpeedArrray[17] = {0XEE, 0XB1, 7, 0, 0, 0, 8, 0, 0, 0, 0, 0, 100, 0XFF, 0XFC, 0XFF, 0XFF};
uint8_t FlowMeterArrray[17] = {0XEE, 0XB1, 7, 0, 0, 0, 9, 0, 0, 0, 0, 0, 100, 0XFF, 0XFC, 0XFF, 0XFF};
uint8_t PressSensor1Arrray[17] = {0XEE, 0XB1, 7, 0, 0, 0, 10, 0, 0, 0, 0, 0, 100, 0XFF, 0XFC, 0XFF, 0XFF};
uint8_t PressSensor2Arrray[17] = {0XEE, 0XB1, 7, 0, 0, 0, 11, 0, 0, 0, 0, 0, 100, 0XFF, 0XFC, 0XFF, 0XFF};
uint8_t PressSensor3Arrray[17] = {0XEE, 0XB1, 7, 0, 0, 0, 12, 0, 0, 0, 0, 0, 100, 0XFF, 0XFC, 0XFF, 0XFF};

uint8_t LeftBatTempArrray[17] = {0XEE, 0XB1, 7, 0, 0, 0, 13, 0, 0, 0, 0, 0, 0, 0XFF, 0XFC, 0XFF, 0XFF};
uint8_t LeftBatCapArrray[17] = {0XEE, 0XB1, 7, 0, 0, 0, 14, 0, 0, 0, 0, 0, 0, 0XFF, 0XFC, 0XFF, 0XFF};

uint8_t RightBatTempArrray[17] = {0XEE, 0XB1, 7, 0, 0, 0, 15, 0, 0, 0, 0, 0, 0, 0XFF, 0XFC, 0XFF, 0XFF};
uint8_t RightBatCapArrray[17] = {0XEE, 0XB1, 7, 0, 0, 0, 16, 0, 0, 0, 0, 0, 0, 0XFF, 0XFC, 0XFF, 0XFF};
static uint8_t ButtonStatearray[12] = {0XEE, 0XB1, 0X10, 0X00, 0X00, 0X00, 0X13, 0X01, 0XFF, 0XFC, 0XFF, 0XFF};
uint8_t ButtonDisable[12] = {0XEE, 0XB1, 0X04, 0X00, 0X00, 0X00, 0X13, 0X00, 0XFF, 0XFC, 0XFF, 0XFF};
void VoltTest()
{

    ADG408A0 = 0;
    ADG408A1 = 0;
    ADG408A2 = 0;
    MPU_5VMPU1 = ADC_GetConversion(0);
    __delay_ms(2);

    ADG408A0 = 1;
    ADG408A1 = 0;
    ADG408A2 = 0;
    MPU_5VMPU2 = ADC_GetConversion(0);
    __delay_ms(2);

    ADG408A0 = 0;
    ADG408A1 = 1;
    ADG408A2 = 0;
    MPU_5VSWC1 = ADC_GetConversion(0);
    __delay_ms(2);

    ADG408A0 = 1;
    ADG408A1 = 1;
    ADG408A2 = 0;
    MPU_5VSWC2 = ADC_GetConversion(0);
    __delay_ms(2);

    ADG408A0 = 0;
    ADG408A1 = 0;
    ADG408A2 = 1;
    MPU_LCD2 = ADC_GetConversion(0);
    __delay_ms(2);

    ADG408A0 = 1;
    ADG408A1 = 0;
    ADG408A2 = 1;
    MPU_LCD1 = ADC_GetConversion(0);
    __delay_ms(2);

    ADG408A0 = 1;
    ADG408A1 = 1;
    ADG408A2 = 1;
    MPU_BOT_FLOW = ADC_GetConversion(0);
    __delay_ms(2);

    SECMOTOR = ADC_GetConversion(1);
    __delay_ms(2);
    MPU_AN2_RBATSW = ADC_GetConversion(2);
    __delay_ms(2);
    VREF_41 = ADC_GetConversion(3);
    __delay_ms(2);
    MPU_AN4_SPKR1 = ADC_GetConversion(4);
    __delay_ms(2);
    MPU_AN5_RBAT = ADC_GetConversion(5);
    __delay_ms(2);
    MPU_AN6_LBATSW = ADC_GetConversion(6);
    __delay_ms(2);
    MPU_AN7_LBAT = ADC_GetConversion(7);
    __delay_ms(2);
    MPU_AN8_VMOTOR = ADC_GetConversion(8);
    __delay_ms(2);
    MPU_AN9_VEXT = ADC_GetConversion(9);
    __delay_ms(2);
    MPU_AN10_SPKR2 = ADC_GetConversion(10);
    __delay_ms(2);
    MPU_AN11_TOP_PSI = ADC_GetConversion(11);
    __delay_ms(2);
    MPU_AN12_ATMOS_PSI = ADC_GetConversion(12);
    __delay_ms(2);
    MPU_AN13_BOT_PSI = ADC_GetConversion(13);
    __delay_ms(2);
    MPU_AN14_5VPSI = ADC_GetConversion(14);
    __delay_ms(2);
    MPU_ISYSTEM = ADC_GetConversion(15);
    __delay_ms(2);
}

void MotorTest()
{
    MotorEnable = 1;
    MOTORENABLE1 = 0;
    MOTORENABLE2 = 0;
}

void SensorTest()
{
    static uint16_t V1, V2, V3;
    static float V1float, V2float, V3float, V4;

    MPU_AN14_5VPSI = ADC_GetConversion(14);
    V4 = (float)MPU_AN14_5VPSI / 200;
    __delay_ms(2);

    V1 = ADC_GetConversion(11);
    // V1 = (V1 * 5 / 409.6);
    V1float = (float)V1 / 100;
    if (V1float > V4)
        MPU_AN11_TOP_PSI = (((V1float - V4)) * 7.5); //(V1 - 0.5) * 7.5;
    else
        MPU_AN11_TOP_PSI = 0;
    //    MPU_AN11_TOP_PSI= V1;
    UART2SendFloat(10, MPU_AN11_TOP_PSI);
    __delay_ms(1);
    V2 = ADC_GetConversion(12);
    V2float = (float)V2 / 100;
    if (V2float > V4)
        MPU_AN12_ATMOS_PSI = (fabs)((V2float - V4) * 7.5); //(V2 - 0.5) * 7.5;
    else
        MPU_AN12_ATMOS_PSI = 0;
    UART2SendFloat(12, MPU_AN12_ATMOS_PSI);
    __delay_ms(1);
    V3 = ADC_GetConversion(13);
    V3float = (float)V3 / 100;
    if (V3float > V4)
        MPU_AN13_BOT_PSI = (fabs)((V3float - V4) * 7.5); //(V3 - 0.5) * 7.5;
    else
        MPU_AN13_BOT_PSI = 0;
    UART2SendFloat(11, MPU_AN13_BOT_PSI);
    __delay_ms(1);

    /* MPU_ISYSTEM = ADC_GetConversion(15);
    MPU_ISYSTEMFloat = ((float)MPU_ISYSTEM / 100 - 4.8) / 0.185;
    UART2SendFloat(28, MPU_ISYSTEMFloat);
    __delay_ms(1); */

    /*     PressSensor1Arrray[7] = 2;
        for (uint8_t i = 8; i < 12; i++)
            PressSensor1Arrray[19 - i] = ((uint8_t *)&MPU_AN11_TOP_PSI)[i - 8];
        UART2SendNByte(PressSensor1Arrray, 17); */

    /*     PressSensor2Arrray[7] = 2;
        for (uint8_t i = 8; i < 12; i++)
            PressSensor2Arrray[19 - i] = ((uint8_t *)&MPU_AN12_ATMOS_PSI)[i - 8];
        UART2SendNByte(PressSensor2Arrray, 17); */

    /*     PressSensor3Arrray[7] = 2;
        for (uint8_t i = 8; i < 12; i++)
            PressSensor3Arrray[19 - i] = ((uint8_t *)&MPU_AN13_BOT_PSI)[i - 8];
        UART2SendNByte(PressSensor3Arrray, 17); */

    // UART2SendNByte(PressSensor1Arrray, 17);
    // UART2SendNByte(PressSensor2Arrray, 17);
    // UART2SendNByte(PressSensor3Arrray, 17);
}

void SpeakerTest()
{
    SPEAKER2 = 0;
    SPEAKER1 = 1;
    __delay_ms(100);
    MPU_AN4_SPKR1 = ADC_GetConversion(4);
    __delay_ms(100);
    SPEAKER1 = 0;
    SPEAKER2 = 1;
    __delay_ms(100);
    MPU_AN10_SPKR2 = ADC_GetConversion(10);
    __delay_ms(100);
}

void FanControl()
{
    FANLOCK = 1;
    FANON = 1;
    __delay_ms(5000);

    FANLOCK = 0;
    FANON = 1;

    __delay_ms(5000);

    FANLOCK = 0;
    FANON = 0;
}

void LeftBatTest()
{
    double LeftBatTempFloat;
    LeftBatCapcity = LeftBatGetCapacity();

    if (LeftBatCapcity >= 2 && LeftBatCapcity <= 100)
        LeftBatCapArrray[12] = LeftBatCapcity;
    else
        LeftBatCapArrray[12] = 0;

    UART2SendNByte(LeftBatCapArrray, 17);
    __delay_ms(2);
    LeftBatTemp = LeftBatGetTemp();
    // LeftBatTempArrray[12] = LeftBatTemp % 256;
    // LeftBatTempArrray[11] = LeftBatTemp / 256;
    if ((LeftBatTemp > 100) && (LeftBatTemp < 1000))
    {

        // UART2SendFloat(13, LeftBatTemp);
        /*         UART2SendByte(0XEE);
                UART2SendByte(0XB1);
                UART2SendByte(0X10);
                UART2SendByte(0X00);
                UART2SendByte(0X00);
                UART2SendByte(0X00);
                UART2SendByte(13);
                UART2SendByte(LeftBatTemp / 100 + 48);
                UART2SendByte((LeftBatTemp / 10) % 10 + 48);
                UART2SendByte(0X2E);
                UART2SendByte(LeftBatTemp % 10 + 48);
                UART2EndSend(); */

        /*         LeftBatTempFloat = LeftBatTemp / 10.0;
                LeftBatTempArrray[7] = 3;
                for (uint8_t i = 8; i < 12; i++)
                    LeftBatTempArrray[20 - i] = ((uint8_t *)&LeftBatTempFloat)[i - 8];
                UART2SendNByte(LeftBatTempArrray, 17); */
    }
    else
        LeftBatTemp = 0;

    UART2SendFloat(13, LeftBatTemp);
}
void RightBatTest()
{
    float RightBatTempFloat;
    RightBatCapcity = RightBatGetCapacity();

    if (RightBatCapcity >= 2 && RightBatCapcity <= 100)
        RightBatCapArrray[12] = RightBatCapcity;
    else
        RightBatCapArrray[12] = 0;

    UART2SendNByte(RightBatCapArrray, 17);
    __delay_ms(2);
    RightBatTemp = RightBatGetTemp();

    if ((RightBatTemp > 100) && (RightBatTemp < 1000))
    {
        // UART2SendFloat(15, RightBatTemp);
        /* RightBatTempFloat = RightBatTemp / 10.0;
        RightBatTempArrray[7] = 2;
        for (uint8_t i = 8; i < 12; i++)
            RightBatTempArrray[19 - i] = ((uint8_t *)&RightBatTempFloat)[i - 8];
        UART2SendNByte(RightBatTempArrray, 17); */
    }
    else
        RightBatTemp = 0;

    UART2SendFloat(15, RightBatTemp);
}

void MotorFreqSend()
{
    // static float LeftTempFloat, RightTempFloat;
    MotorFreq = 0;
    for (int i = 0; i < 3; i++)
    {
        MotorFreq += MotorFreqArray[i];
    }
    MotorFreq = MotorFreq / 3;

    MotorSpeedArrray[11] = MotorFreq / 256;
    MotorSpeedArrray[12] = MotorFreq % 256;
    UART2SendNByte(MotorSpeedArrray, 17);

    /*     static float Motorspeed1 = 125.4;
        static uint8_t len;
        // unsigned char TransmitData[20];
        char *TransmitData = NULL;

        UART2BeginSend();

        UART2SendSCRIDCONID(8);
        UART2SendByte(0X00);
        sprintf(TransmitData, "%.1f\n", Motorspeed1); // MotorFreq); //.1f
        len = strlen(TransmitData);
        UART2SendByte(len);
        UART2SendNByte(TransmitData, len);

        UART2SendSCRIDCONID(10);
        UART2SendByte(0X00);
        sprintf(TransmitData, "%.1f\n", MPU_AN11_TOP_PSI); //.1f
        len = strlen(TransmitData);
        UART2SendByte(len);
        UART2SendNByte(TransmitData, len);

        UART2SendSCRIDCONID(11);
        UART2SendByte(0X00);
        sprintf(TransmitData, "%.1f\n", MPU_AN12_ATMOS_PSI); //.1f
        len = strlen(TransmitData);
        UART2SendByte(len);
        UART2SendNByte(TransmitData, len);

        UART2SendSCRIDCONID(12);
        UART2SendByte(0X00);
        sprintf(TransmitData, "%.1f\n", MPU_AN13_BOT_PSI); //.1f
        len = strlen(TransmitData);
        UART2SendByte(len);
        UART2SendNByte(TransmitData, len);

        if (LeftBatCapcity >= 0 && LeftBatCapcity <= 100)
        {
            UART2SendSCRIDCONID(14);
            UART2SendByte(0X00);
            sprintf(TransmitData, "%d", LeftBatCapcity); //.1f
            len = strlen(TransmitData);
            UART2SendByte(len);
            UART2SendNByte(TransmitData, len);
        } */

    /*     if ((LeftBatTemp > 100) && (LeftBatTemp < 1000))
        {
            LeftTempFloat = LeftBatTemp / 10;
            UART2SendSCRIDCONID(13);
            UART2SendByte(0X00);
            sprintf(TransmitData, "%.1f\n", LeftTempFloat); //.1f
            len = strlen(TransmitData);
            UART2SendByte(len);
            UART2SendNByte(TransmitData, len);
        } */

    /*     if (RightBatCapcity >= 0 && RightBatCapcity <= 100)
        {
            UART2SendSCRIDCONID(16);
            UART2SendByte(0X00);
            sprintf(TransmitData, "%d\n", RightBatCapcity); //.1f
            len = strlen(TransmitData);
            UART2SendByte(len);
            UART2SendNByte(TransmitData, len);
        }

        if ((RightBatTemp > 100) && (RightBatTemp < 1000))
        {
            RightTempFloat = RightBatTemp / 10;
            UART2SendSCRIDCONID(15);
            UART2SendByte(0X00);
            sprintf(TransmitData, "%.1f\n", RightTempFloat); //.1f
            len = strlen(TransmitData);
            UART2SendByte(len);
            UART2SendNByte(TransmitData, len);
        }
     */
    // UART2EndSend();
}

void FlowMeterDet()
{
    static float MPU_ISYSTEMFloat;
    __delay_ms(2);
    MPU_ISYSTEM = ADC_GetConversion(15);
    MPU_ISYSTEMFloat = ((float)MPU_ISYSTEM / 100 - 4.8) / 0.185;
    UART2SendFloat(28, MPU_ISYSTEMFloat);
    __delay_ms(20);
    /*     static uint16_t FlowMeter;

        ADG408A0 = 1;
        ADG408A1 = 1;
        ADG408A2 = 1;
        __delay_ms(2);
        MPU_BOT_FLOW = ADC_GetConversion(0);
        FlowMeter = (float)MPU_BOT_FLOW / 100;
        if (FlowMeter > 5)
            FlowMeter = 0.003 * FlowMeter * FlowMeter * FlowMeter - 0.104 * FlowMeter * FlowMeter + 1.355 * FlowMeter - 4.531;
        else
            FlowMeter = 0;

        UART2SendFloat(9, FlowMeter); */

    //   FlowMeter =
    /*   FlowMeterArrray[7] = 2;
      for (uint8_t i = 8; i < 12; i++)
          FlowMeterArrray[19 - i] = ((uint8_t *)&FlowMeter)[i - 8];

      UART2SendNByte(FlowMeterArrray, 17); */
}

void ProcessReceivedFrame(uint8_t *frame, uint8_t length)
{
    static uint16_t voltageRaw1, voltageRaw2, voltageRaw3;
    // static uint8_t ButtonGroupState[] = {0XEE, 0XB1, 0X12, 0X00, 0X00, 0X00, 0X13, 0X00, 0X01, 0X01, 0X00, 0X14, 0X00, 0X01, 0X01, 0X00, 0X15, 0X00, 0X01, 0X01, 0X00, 0X17, 0X00, 0X01, 0X01, 0X00, 0X18, 0X00, 0X01, 0X01, 0XFF, 0XFC, 0XFF, 0XFF};

    if (UartReceiveOKFlag == 1)
    {
        UartReceiveOKFlag = 0;
        rxState = RX_IDLE;
        Usart1RecCount = 0;

        if (length != 10)
            return;

        voltageRaw1 = ((uint16_t)frame[3] << 8) | frame[4]; // d0 + d1
        voltageRaw2 = ((uint16_t)frame[5] << 8) | frame[6]; // d2 + d3
        voltageRaw3 = ((uint16_t)frame[7] << 8) | frame[8]; // d4 + d5

        PriSpeedV1 = ((uint32_t)voltageRaw1 & 0x0FFF) * 25 / 4096;
        SlaSpeedV1 = ((uint32_t)voltageRaw2 & 0x0FFF) * 25 / 4096;
        InputPowerCurrent = (voltageRaw3 & 0x0FFF) * 20 / 4096; //=V*25/(4096*50*0.025)
        PriSpeedV = PriSpeedV1;
        SlaSpeedV = SlaSpeedV1;
        UART2SendFloat(8, PriSpeedV);
        UART2SendFloat(9, SlaSpeedV);
        // UART2SendFloat(28, InputPowerCurrent);

        statusByte = frame[9];
        TDC5VState = (statusByte >> 0) & 0x01;
        SlaVState = (statusByte >> 1) & 0x01;
        PriEnState = (statusByte >> 2) & 0x01;
        PriVState = (statusByte >> 3) & 0x01;

        if (OldPriVState != PriVState)
        {
            ButtonStatearray[6] = 19;
            ButtonStatearray[7] = PriVState;
            UART2SendNByte(ButtonStatearray, 12);
            OldPriVState = PriVState;
            __delay_ms(2);
        }

        if (OldPriEnState != PriEnState)
        {
            ButtonStatearray[6] = 20;
            ButtonStatearray[7] = PriEnState;
            UART2SendNByte(ButtonStatearray, 12);
            OldPriEnState = PriEnState;
            __delay_ms(2);
        }

        if (OldSlaVState != SlaVState)
        {
            ButtonStatearray[6] = 21;
            ButtonStatearray[7] = SlaVState;
            UART2SendNByte(ButtonStatearray, 12);
            OldSlaVState = SlaVState;
            __delay_ms(2);
        }

        if (OldTDC5VState != TDC5VState)
        {
            ButtonStatearray[6] = 25;
            ButtonStatearray[7] = TDC5VState;
            UART2SendNByte(ButtonStatearray, 12);
            OldTDC5VState = TDC5VState;
            __delay_ms(2);
        }
    }

    if (PORTBbits.RB0 == 0)
        BDCState = 1;
    else
        BDCState = 0;

    if (PORTBbits.RB1 == 0)
        TDCState = 1;
    else
        TDCState = 0;

    if (OldTDCState != TDCState)
    {
        ButtonStatearray[6] = 23;
        ButtonStatearray[7] = TDCState;
        UART2SendNByte(ButtonStatearray, 12);
        OldTDCState = TDCState;
        __delay_ms(2);
    }

    if (OldBDCState != BDCState)
    {
        ButtonStatearray[6] = 24;
        ButtonStatearray[7] = BDCState;
        UART2SendNByte(ButtonStatearray, 12);
        OldBDCState = BDCState;
        __delay_ms(2);
    }
}
void LM73Read()
{
    LM73Temperature1 = LM73Read1();
    LM73Temperature2 = LM73Read2();

    UART2SendFloat(26, LM73Temperature1);
    UART2SendFloat(27, LM73Temperature2);
}
void LCDInit(void)
{
    for (uint8_t i = 19; i <= 25; i++)
    {
        ButtonStatearray[6] = i;
        ButtonStatearray[7] = 0;
        UART2SendNByte(12, ButtonStatearray);
        __delay_ms(2);
        ButtonDisable[6] = i;
        UART2SendNByte(12, ButtonDisable);
        __delay_ms(2);
    }
}

void LCDLEDBlink()
{
    LBATID2 = 1;
    LEDFAULT2 = 1;
    RBATID2 = 1;
    LBATID1 = 1;
    LEDFAULT1 = 1;
    RBATID1 = 1;
    __delay_ms(500);
    LBATID2 = 0;
    LEDFAULT2 = 0;
    RBATID2 = 0;
    LBATID1 = 0;
    LEDFAULT1 = 0;
    RBATID1 = 0;
    __delay_ms(500);
    LBATID2 = 1;
    LEDFAULT2 = 1;
    RBATID2 = 1;
    LBATID1 = 1;
    LEDFAULT1 = 1;
    RBATID1 = 1;
    __delay_ms(500);
    LBATID2 = 0;
    LEDFAULT2 = 0;
    RBATID2 = 0;
    LBATID1 = 0;
    LEDFAULT1 = 0;
    RBATID1 = 0;
}
